#include "data_manage.h"
#include "nrf_soc.h"
#include "app_error.h"
#include "log.h"
#include "inv_mpu.h"
#include "nrf_delay.h"

#include "algorithm_app.h"

static uint8_t m_data_out_mode = DATA_FOR_CAPTURE;

static data_manage_t conf_data;

void set_acc_gyro_to_conf_data(long *acc, long *gyro)
{
    for(int i = 0; i < 3; i++)
    {
        conf_data.acc_bias[i] = (uint32_t)acc[i];
        conf_data.gyro_bias[i] = (uint32_t)gyro[i];
    }
}

void set_frequence_to_conf_data(uint32_t freq)
{
    conf_data.frequence = freq;
}

void save_data(void)
{
    uint32_t err_code;
    
    err_code = sd_flash_page_erase(CONFIG_FILE_FAGE);
    APP_ERROR_CHECK(err_code);
    
    nrf_delay_ms(500);
    
    err_code = sd_flash_write((uint32_t *)(1024 * CONFIG_FILE_FAGE), (uint32_t *)&conf_data, sizeof(data_manage_t) / sizeof(uint32_t));
    LOG_X("save_offset_data: %X", err_code);
    LOG_X("Sava: %d,%d", (long)conf_data.acc_bias[0], (long)conf_data.gyro_bias[0]);
    APP_ERROR_CHECK(err_code);
}

static void flash_read(uint32_t * const addr, uint32_t * des, uint32_t size)
{
    for(int i = 0; i < size; i++)
    {
        *(des + i) = *(addr + i);
    }
}

void load_data(void)
{
    long acc[3], gyro[3];
    
    flash_read((uint32_t *)(1024 * CONFIG_FILE_FAGE), (uint32_t *)&conf_data, sizeof(data_manage_t) / sizeof(uint32_t));
    LOG_X("Load: %d, %d, %d", (long)conf_data.acc_bias[0], (long)conf_data.gyro_bias[0], (long)conf_data.frequence);
    
    for(int i = 0; i < 3; i++)
    {
        acc[i] = (long)conf_data.acc_bias[i];
        gyro[i] = (long)conf_data.gyro_bias[i];
    }
    
    mpu_set_gyro_bias_reg(gyro);
    mpu_set_accel_bias_6500_reg(acc);
}


/**
 *  @brief        set data out mode
 *  @param[in]    Data_out_mode
 *  @return       NULL
 */
void set_data_out_mode(Date_out_mode mode)
{
    m_data_out_mode = mode;
}


/**
 *  @brief        get data out mode
 *  @param[in]    NULL
 *  @return       DATA_FOR_ANO, DATA_FOR_CAPTURE
 */
Date_out_mode get_data_out_mode(void)
{
    return (Date_out_mode)m_data_out_mode;
}


void save_data_out_mode(void)
{
    uint32_t err_code;
    uint32_t mode;
    
    err_code = sd_flash_page_erase(CONFIG_DATA_OUT_MODE_PAGE);   
    APP_ERROR_CHECK(err_code);
    
    nrf_delay_ms(500);
    
    mode = (uint32_t)m_data_out_mode;
    
    err_code = sd_flash_write((uint32_t *)(PAGE_SIZE * CONFIG_DATA_OUT_MODE_PAGE), (uint32_t *)&mode, sizeof(mode) / sizeof(uint32_t));
    LOG_X("save_data_out_mode: %X", err_code);
    APP_ERROR_CHECK(err_code);

}


void load_data_out_mode(void)
{
    uint32_t mode;
    
    flash_read((uint32_t *)(PAGE_SIZE * CONFIG_DATA_OUT_MODE_PAGE), (uint32_t *)&mode, sizeof(mode) / sizeof(uint32_t));
    LOG_X("Data out Mode:%X", mode);
    
    if(mode != 0 && mode != 1)
    {
        m_data_out_mode = DATA_FOR_CAPTURE;
        return;
    }
    
    m_data_out_mode = (uint8_t)mode;
    
    LOG_X("data out");
    
}



void save_app_type(uint32_t index)
{
	uint32_t err_code, tmp;
	uint32_t type = index;
	
	set_algorithm_app_type(index);
	
	flash_read((uint32_t *)(PAGE_SIZE * CONFIG_GAME_INDEX), (uint32_t *)&tmp, 1);
	
	if(tmp == index)
	{
		LOG_X("Config repeat");
		return;
	}
	
	LOG_X("Prepare to save app type");
	
    err_code = sd_flash_page_erase(CONFIG_GAME_INDEX);   
    APP_ERROR_CHECK(err_code);
    
    nrf_delay_ms(500);
    
    err_code = sd_flash_write((uint32_t *)(PAGE_SIZE * CONFIG_GAME_INDEX), (uint32_t *)&type, 1);
    LOG_X("save_game_index:%d, err_code:%X", index, err_code);
    APP_ERROR_CHECK(err_code);
}

void load_app_type(void)
{
	uint32_t index;
	flash_read((uint32_t *)(PAGE_SIZE * CONFIG_GAME_INDEX), (uint32_t *)&index, 1);
	if(index == 0xffffffff)
		index = SIM_DRIVE;		// set first game index
	
	set_algorithm_app_type(index);
    LOG_X("Game index:%X", index);
}

